<html>
  <head>
  <meta charset="utf-8">
   <meta http-equiv="X-UA-Compatible" content="IE=edge">
   <meta name="viewport" content="width=device-width, initial-scale=1">
   <meta name="theme-color" content="#008000"/>
   <title>pathfinding_ml</title>
   <link rel="manifest" href="pathfinding_ml_manifest.json">
   <script>
    if('serviceWorker' in navigator) {
      navigator.serviceWorker.register('/pathfinding_ml_sw.js')
        .then(function() {
              console.log('Service Worker Registered');
        });
    }
   </script>
    <style>
      body {
        margin: 0;
      }
    </style>
  </head>
  <body>
  <h1>pathfinding_ml</h1>
  <script src="three.js"></script>
  <script src="skin.js"></script>
  <script src="astar.js"></script>
  <script>
    let container, scene, camera, session, model, avatarMesh;
    let mesher = null;
    const waypoints = {};
    let numWaypoints = 0;
    let waypointsValid = false;
    const lineMeshes = [];
    let animation = null;

    const localVector = new THREE.Vector3();
    const localVector2 = new THREE.Vector3();
    const localVector3 = new THREE.Vector3();
    const localQuaternion = new THREE.Quaternion();
    const localEuler = new THREE.Euler();
    const localMatrix = new THREE.Matrix4();
    const localFloat32Array = new Float32Array(16);

    const waypointResolution = 0.2;
    // const waypointResolutionSq = waypointResolution*waypointResolution;

    const numCubes = 100;
    // const cubeSize = 0.2;
    const cubeGeometry = new THREE.BoxBufferGeometry(1, 1, 1);
    const pointGeometry = cubeGeometry.clone().applyMatrix(new THREE.Matrix4().makeScale(0.01, 0.01, 0.01));
    const pointMaterial = new THREE.MeshPhongMaterial({
      color: 0x42a5f5,
    });
    const lineGeometry = cubeGeometry.clone().applyMatrix(new THREE.Matrix4().makeScale(0.01/2, 0.01/2, 1));
    const lineMaterial = new THREE.MeshPhongMaterial({
      color: 0xFFFFFF,
    });
    let pathValid = true;

    function init() {
      container = document.createElement('div');
      document.body.appendChild(container);

      scene = new THREE.Scene();
      scene.matrixAutoUpdate = false;
      // scene.background = new THREE.Color(0x3B3961);

      camera = new THREE.PerspectiveCamera(60, window.innerWidth / window.innerHeight, 0.1, 1000);
      camera.position.set(0, 0, 1);
      // camera.lookAt(new THREE.Vector3());
      scene.add(camera);

      const ambientLight = new THREE.AmbientLight(0x808080);
      scene.add(ambientLight);

      const directionalLight = new THREE.DirectionalLight(0xFFFFFF, 1);
      directionalLight.position.set(1, 1, 1);
      scene.add(directionalLight);

      renderer = new THREE.WebGLRenderer({
        antialias: true,
        alpha: true,
      });
      renderer.setPixelRatio(window.devicePixelRatio);
      renderer.setSize(window.innerWidth, window.innerHeight);

      // window.browser.magicleap.RequestDepthPopulation(true);
      // renderer.autoClear = false;

      container.appendChild(renderer.domElement);

      avatarMesh = (() => {
        const DEFAULT_SKIN_URL = 'skin.png';

        const mesh = skin({
          limbs: true,
        });
        mesh.scale.multiplyScalar(0.2);
        mesh.frustumCulled = false;
        mesh.node = null;

        new Promise((accept, reject) => {
          const skinImg = new Image();
          skinImg.crossOrigin = 'Anonymous';
          skinImg.src = DEFAULT_SKIN_URL;
          skinImg.onload = () => {
            accept(skinImg);
          };
          skinImg.onerror = err => {
            reject(err);
          };
        })
          .then(skinImg => {
            mesh.setImage(skinImg);
          });

        return mesh;
      })();
      scene.add(avatarMesh);

      const planeMeshes = [];
      const planeMeshesCache = {};
      const planeGeometry1 = new THREE.PlaneBufferGeometry(1, 1)
        .applyMatrix(
          new THREE.Matrix4().makeRotationFromQuaternion(
            new THREE.Quaternion().setFromUnitVectors(
              new THREE.Vector3(0, 0, -1),
              new THREE.Vector3(0, 1, 0),
            )
          )
        );
      const planeMaterial1 = new THREE.MeshPhongMaterial({
        color: 0xe91e63,
      });
      const planeGeometry2 = planeGeometry1.clone()
        .applyMatrix(
          new THREE.Matrix4().makeRotationFromQuaternion(
            new THREE.Quaternion().setFromUnitVectors(
              new THREE.Vector3(0, 1, 0),
              new THREE.Vector3(0, -1, 0),
            )
          )
        );
      const planeMaterial2 = new THREE.MeshPhongMaterial({
        color: 0x673ab7,
      });
      const _loadPlanes = planes => {
        _clearPlanes();

        for (let i = 0; i < planes.length; i++) {
          const plane = planes[i];

          const object = (() => {
            const cachedObject = planeMeshesCache[plane.id];

            if (cachedObject) {
              return cachedObject;
            } else {
              const object = new THREE.Object3D();
              object.position.fromArray(plane.position);
              object.quaternion.fromArray(plane.rotation);

              const frontMesh = (() => {
                const mesh = new THREE.Mesh(planeGeometry1, planeMaterial1);
                mesh.scale.set(plane.size[0], 1, plane.size[1]);
                mesh.frustumCulled = false;
                // mesh.updateMatrix();
                // mesh.updateMatrixWorld();
                return mesh;
              })();
              object.add(frontMesh);
              object.frontMesh = frontMesh;

              const backMesh = (() => {
                const mesh = new THREE.Mesh(planeGeometry2, planeMaterial2);
                mesh.scale.set(plane.size[0], 1, plane.size[1]);
                mesh.frustumCulled = false;
                // mesh.updateMatrix();
                // mesh.updateMatrixWorld();
                return mesh;
              })();
              object.add(backMesh);
              object.backMesh = backMesh;

              planeMeshesCache[plane.id] = object;

              return object;
            }
          })();

          scene.add(object);
          planeMeshes.push(object);
        }

        if (!waypointsValid) {
          for (let i = 0; i < planeMeshes.length; i++) {
            const planeMesh = planeMeshes[i];
            const {position, quaternion} = planeMesh;
            const {frontMesh} = planeMesh;
            const {scale} = frontMesh;

            for (let x = Math.ceil(-scale.x/2/waypointResolution)*waypointResolution; x < scale.x/2; x += waypointResolution) {
              for (let z = Math.ceil(-scale.z/2/waypointResolution)*waypointResolution; z < scale.z/2; z += waypointResolution) {
                const waypoint = position.clone()
                  .add(
                    localVector.set(x, waypointResolution/2, z)
                      .applyQuaternion(quaternion)
                  );
                waypoint.x = Math.floor(waypoint.x/waypointResolution)*waypointResolution;
                waypoint.y = Math.floor(waypoint.y/waypointResolution)*waypointResolution;
                waypoint.z = Math.floor(waypoint.z/waypointResolution)*waypointResolution;

                waypoint.id = (++numWaypoints) + '';
                waypoint.neighbors = [];
                waypoint.island = null;

                const key = `${waypoint.x.toFixed(5)}:${waypoint.y.toFixed(5)}:${waypoint.z.toFixed(5)}`;
                waypoints[key] = waypoint;

                const pointMesh = new THREE.Mesh(pointGeometry, pointMaterial);
                pointMesh.position.copy(waypoint);
                pointMesh.frustumCulled = false;
                scene.add(pointMesh);
              }
            }
          }

          for (const key in waypoints) {
            const waypoint = waypoints[key];

            for (let dz = -1; dz <= 1; dz++) {
              for (let dy = -1; dy <= 1; dy++) {
                for (let dx = -1; dx <= 1; dx++) {
                  if (dz !== 0 || dy !== 0 || dx !== 0) {
                    const key = `${(waypoint.x + dx*waypointResolution).toFixed(5)}:${(waypoint.y + dy*waypointResolution).toFixed(5)}:${(waypoint.z + dz*waypointResolution).toFixed(5)}`;
                    const neighbor = waypoints[key];
                    if (neighbor) {
                      waypoint.neighbors.push(neighbor);
                    }
                  }
                }
              }
            }
          }

          for (const key in waypoints) {
            const waypoint = waypoints[key];

            if (!waypoint.island) {
              const island = [];
              const set = new Set();
              const queue = [waypoint];

              while (queue.length > 0) {
                const waypoint = queue.pop();
                island.push(waypoint);
                waypoint.island = island;

                const {neighbors} = waypoint;
                for (let i = 0; i < neighbors.length; i++) {
                  const neighbor = neighbors[i];

                  if (!neighbor.island) {
                    queue.push(neighbor);
                  }
                }
              }
            } else {
              continue;
            }
          }

          waypointsValid = true;
        }
      };
      const _clearPlanes = () => {
        for (let i = 0; i < planeMeshes.length; i++) {
          scene.remove(planeMeshes[i]);
        }
        planeMeshes.length = 0;
      };
      const _onPlanes = planes => {
        _loadPlanes(planes);
      };

      if (window.browser && window.browser.magicleap) {
        planeTracker = window.browser.magicleap.RequestPlaneTracking();
        planeTracker.onplanes = _onPlanes;
      } else {
        const updates = Array(10);
        const _makeSize = () => new THREE.Vector2(1 + Math.random()*2, 1 + Math.random()*2);
        const _makeOffset = (oldPosition, oldNormal, oldSize) => {
          const corner = (() => {
            const index = Math.floor(Math.random() * 4);
            switch (index) {
              case 0: return new THREE.Vector3(1, 0, 0);
              case 1: return new THREE.Vector3(-1, 0, 0);
              case 2: return new THREE.Vector3(0, 0, 1);
              case 3: return new THREE.Vector3(0, 0, -1);
              default: return null;
            }
          })();
          const direction = Math.random() < 0.5 ? 1 : -1;

          const newNormal = corner.clone()
            .applyQuaternion(
              new THREE.Quaternion().setFromUnitVectors(
                new THREE.Vector3(0, 1, 0),
                oldNormal
              )
            )
            .multiplyScalar(direction);
          const newSize = _makeSize();
          const newPosition = oldPosition.clone()
            .add(
              corner.clone()
                .multiply(new THREE.Vector3(oldSize.x/2, 1, oldSize.y/2))
                .applyQuaternion(
                  new THREE.Quaternion().setFromUnitVectors(
                    new THREE.Vector3(0, 1, 0),
                    oldNormal
                  )
                )
            )
            .sub(
              oldNormal.clone()
                .multiplyScalar(direction)
                .multiply(
                  new THREE.Vector3(newSize.x/2, 1, newSize.y/2)
                    .applyQuaternion(
                      new THREE.Quaternion().setFromUnitVectors(
                        new THREE.Vector3(0, 1, 0),
                        newNormal
                      )
                    )
                )
            );

          return {
            position: newPosition,
            normal: newNormal,
            size: newSize,
          };
        };
        let position = null;
        let normal = null;
        let size = null;
        for (let i = 0; i < updates.length; i++) {
          if (!position) {
            position = new THREE.Vector3(0, 0, 0);
            normal = new THREE.Vector3(0, 1, 0);
            size = _makeSize();
          } else {
            const offset = _makeOffset(position, normal, size);

            position = offset.position;
            normal = offset.normal;
            size = offset.size;
          }
          updates[i] = {
            id: i + '',
            position: position.toArray(new Float32Array(3)),
            rotation: new THREE.Quaternion().setFromUnitVectors(
              new THREE.Vector3(0, 1, 0),
              normal
            ).toArray(new Float32Array(4)),
            size: size.toArray(new Float32Array(2)),
          };
        }
        setInterval(() => {
          _onPlanes(updates);
        }, 100);
      }

      window.addEventListener('keydown', e => {
        if (e.keyCode === 13) { // enter
          if (enabled) {
            _disable();
          } else {
            _enable();
          }
        }
      });

      renderer.setAnimationLoop(animate);
    }

    let path = null;
    let pathIndex = 0;
    const thetaRate = 200;
    const animationTime = 50;
    function animate(time, frame) {
      if (model) {
        // const animationTime = 4000;
        const f = ((Date.now() % animationTime) / animationTime) * (Math.PI * 2);
        model.quaternion.setFromUnitVectors(
          new THREE.Vector3(0, 0, 1),
          new THREE.Vector3(Math.cos(f), 0, Math.sin(f)).normalize()
        );
        model.updateMatrixWorld();
      }
      
      if (waypointsValid) {
        const _getRandomWaypoint = () => {
          const keys = Object.keys(waypoints);
          const key = keys[Math.floor(Math.random() * keys.length)];
          return waypoints[key];
        };
        const _getFurthestWaypoint = (startNode, position) => {
          const {island} = startNode;

          let result = island[0];
          let resultDistanceSq = result.distanceToSquared(position);
          for (let i = 1; i < island.length; i++) {
            const waypoint = island[i];
            const distanceSq = waypoint.distanceToSquared(position);
            if (distanceSq < resultDistanceSq) {
              result = waypoint;
              resultDistanceSq = distanceSq;
            }
          }
          return result;
        };
        const _getPath = (waypoint1, waypoint2) => {
          const path = astar(waypoint1, waypoint2, {
            id (node) {
              return node.id;
            },
            isGoal(node) {
              return node === waypoint2;
            },
            getSuccessors(node) {
              return node.neighbors;
            },
            distance(nodeA, nodeB) {
              return 1;
            },
            estimate(node, goal) {
              return node.distanceToSquared(goal);
            }
          });

          if (path.length > 1) {
            return path;
          } else {
            return null;
          }
        };
        const _highlightPath = path => {
          for (let i = 0; i < lineMeshes.length; i++) {
            scene.remove(lineMeshes[i]);
          }
          lineMeshes.length = 0;

          for (let i = 0; i < path.length - 1; i++) {
            const waypoint1 = path[i];
            const waypoint2 = path[i+1];

            const lineMesh = new THREE.Mesh(lineGeometry, lineMaterial);
            lineMesh.position
              .copy(waypoint1)
              .add(waypoint2)
              .divideScalar(2);
            lineMesh.quaternion.setFromUnitVectors(
              localVector.set(0, 0, -1),
              localVector2.copy(waypoint2).sub(waypoint1).normalize()
            );
            lineMesh.scale.z = waypoint1.distanceTo(waypoint2) * 0.8;
            lineMesh.frustumCulled = false;
            scene.add(lineMesh);

            lineMeshes.push(lineMesh);
          }
        };

        if (!path) {
          if (!avatarMesh.node) {
            avatarMesh.node = _getRandomWaypoint();
          }
          const startNode = avatarMesh.node;
          let endNode = null;
          {
            const vrCamera = renderer.vr.enabled ? renderer.vr.getCamera(camera) : camera;
            vrCamera.matrixWorld.decompose(localVector, localQuaternion, localVector2);

            endNode = _getFurthestWaypoint(startNode, localVector);
            path = _getPath(startNode, endNode);
          }
          if (path) {
            _highlightPath(path);
            pathIndex = 0;
          }
        }
        if (path && !animation) {
          const now = Date.now();
          animation = {
            startTime: now,
            endTime: now + animationTime,
            startPosition: avatarMesh.node
              .clone(),
              // .add(localVector2.set(cubeSize/2, cubeSize/2, cubeSize/2)),
            startQuaternion: avatarMesh.quaternion.clone(),
            update() {
              const now = Date.now();
              const factor = Math.min((now - this.startTime) / (this.endTime - this.startTime), 1);
              const nextPathIndex = pathIndex + 1;
              const {startPosition} = this;
              const endPosition = localVector.copy(path[nextPathIndex])//.add(localVector2.set(cubeSize/2, cubeSize/2, cubeSize/2));
              avatarMesh.position
                .copy(startPosition)
                .lerp(endPosition, factor);

              const {startQuaternion} = this;
              localEuler.setFromQuaternion(
                localQuaternion.setFromUnitVectors(
                  localVector2.set(0, 0, -1),
                  localVector3
                    .copy(endPosition)
                    .sub(startPosition)
                    .normalize()
                ),
                'YXZ'
              );
              localEuler.x = localEuler.z = 0;
              const endQuaternion = localQuaternion.setFromEuler(localEuler);
              avatarMesh.quaternion
                .copy(startQuaternion)
                .slerp(endQuaternion, factor);

              avatarMesh.material.uniforms.theta.value = Math.sin((now%thetaRate)/thetaRate * Math.PI*2);

              if (factor >= 1) {
                avatarMesh.node = path[nextPathIndex];
                animation = null;

                if ((nextPathIndex + 1) < path.length) {
                  pathIndex = nextPathIndex;
                } else {
                  path = null;
                }
              }
            },
          };
        }
        if (animation) {
          animation.update();
        }
      }

      renderer.render(scene, renderer.vr.enabled ? renderer.vr.getCamera(camera) : camera);
    }

    init();

    (async () => {
      console.log('request session');
      session = await navigator.xr.requestSession({
        exclusive: true,
      }).catch(err => Promise.resolve(null));

      if (session) {
        // console.log('request first frame');
        session.requestAnimationFrame((timestamp, frame) => {
          renderer.vr.setSession(session, {
            frameOfReferenceType: 'stage',
          });

          const {views} = frame.getViewerPose();
          const viewport = session.baseLayer.getViewport(views[0]);
          const width = viewport.width;
          const height = viewport.height;

          renderer.setSize(width * 2, height);

          renderer.setAnimationLoop(null);

          renderer.vr.enabled = true;
          renderer.vr.setAnimationLoop(animate);

          console.log('running!');
        });
      } else {
        console.log('no xr devices');
      }
    })();

    renderer.setAnimationLoop(animate);
  </script>
  </body>
</html>
